#include <fmt/core.h>
#include <string>
#include <atomic>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <queue>

#include <chrono>
#include <nlohmann/json.hpp>
#include <opencv2/opencv.hpp>
#include "io/camera.hpp"
#include "serial/serial.h"
#include "tasks/auto_aim/aimer.hpp"
#include "tasks/auto_aim/solver.hpp"
#include "tasks/auto_aim/tracker.hpp"
#include "tasks/auto_aim/yolo.hpp"
#include "tools/exiter.hpp"
#include "tools/img_tools.hpp"
#include "tools/logger.hpp"
#include "tools/math_tools.hpp"
#include "tools/plotter.hpp"

// 线程安全队列模板
template<typename T>
class ThreadSafeQueue {
private:
    std::queue<T> queue_;
    mutable std::mutex mutex_;
    std::condition_variable cond_;
    size_t max_size_;
    
public:
    ThreadSafeQueue(size_t max_size = 10) : max_size_(max_size) {}
    
    void push(T value) {
        std::lock_guard<std::mutex> lock(mutex_);
        if (queue_.size() >= max_size_) {
            queue_.pop(); // 如果队列满，丢弃最旧的数据
        }
        queue_.push(std::move(value));
        cond_.notify_one();
    }
    
    bool pop(T& value) {
        std::unique_lock<std::mutex> lock(mutex_);
        if (cond_.wait_for(lock, std::chrono::milliseconds(1), 
                          [this] { return !queue_.empty(); })) {
            value = std::move(queue_.front());
            queue_.pop();
            return true;
        }
        return false;
    }
    
    bool empty() const {
        std::lock_guard<std::mutex> lock(mutex_);
        return queue_.empty();
    }
    
    size_t size() const {
        std::lock_guard<std::mutex> lock(mutex_);
        return queue_.size();
    }
};

// 图像数据和时间戳结构
struct ImageData {
    cv::Mat image;
    std::chrono::steady_clock::time_point timestamp;
    int frame_id;
};

// 检测结果结构
struct DetectionResult {
    std::list<auto_aim::Armor> armors;
    std::chrono::steady_clock::time_point timestamp;
    int frame_id;
};

// 跟踪结果结构
struct TrackingResult {
    std::list<auto_aim::Target> targets;
    std::chrono::steady_clock::time_point timestamp;
    int frame_id;
};

// 瞄准结果结构
struct AimingResult {
    io::Command command;
    std::chrono::steady_clock::time_point timestamp;
    int frame_id;
};

// 串口数据包结构
struct SerialPacket {
    double delta_yaw;
    double delta_pitch;
    double horizon_distance;
    bool control;
    bool shoot;
    std::chrono::steady_clock::time_point timestamp;
    int frame_id;
};

int main(int argc, char * argv[])
{
    // 手动解析命令行参数
    std::string config_path = "configs/sentry.yaml";
    
    for (int i = 1; i < argc; i++) {
        std::string arg = argv[i];
        
        if (arg == "-h" || arg == "--help" || arg == "-?" || arg == "--usage") {
            std::cout << "用法: " << argv[0] << " [配置文件路径]" << std::endl;
            std::cout << "参数说明:" << std::endl;
            std::cout << "  配置文件路径: yaml配置文件路径 (默认: configs/sentry.yaml)" << std::endl;
            return 0;
        }
        else if (arg[0] != '-') {
            config_path = arg;
        }
    }

    tools::logger()->info("Starting parallel pipeline USB camera mode with config: {}", config_path);
    tools::Exiter exiter;

    // 初始化摄像头
    io::Camera camera(config_path);

    // 初始化串口
    serial::Serial serial_port;
    try {
        serial_port.setPort("/dev/ttyACM0");
        serial_port.setBaudrate(115200);
        serial_port.setTimeout(serial::Timeout::max(), 10, 0, 10, 0); // 减少超时
        serial_port.open();
        tools::logger()->info("Serial port /dev/ttyACM0 opened successfully");
    } catch (const std::exception & e) {
        tools::logger()->error("Failed to open serial port: {}", e.what());
        return -1;
    }

    // 初始化YOLO、Solver、Tracker、Aimer（重新启用模型检测）
    auto_aim::YOLO yolo(config_path);
    auto_aim::Solver solver(config_path);
    auto_aim::Tracker tracker(config_path, solver);
    auto_aim::Aimer aimer(config_path);

    // 创建线程安全队列（减少队列深度，降低延迟）
    ThreadSafeQueue<ImageData> image_queue(2);        // 图像队列，减少到2帧
    ThreadSafeQueue<SerialPacket> serial_queue(5);      // 串口数据队列，减少到5个包
    
    // 原子变量控制线程退出
    std::atomic<bool> quit{false};
    
    // 四元数数据（使用原子变量，线程安全）
    std::atomic<double> w{1.0}, x{0.0}, y{0.0}, z{0.0};
    
    // 性能统计
    std::atomic<int> camera_frame_count{0};
    std::atomic<int> detection_frame_count{0};
    std::atomic<int> serial_frame_count{0};
    std::atomic<int> display_frame_count{0};
    
    auto start_time = std::chrono::steady_clock::now();
    int global_frame_id = 0;

    // 线程1：摄像头读取线程（仅模型检测，不包含串口发送）
    auto camera_thread = std::thread([&]() {
        tools::logger()->info("Camera thread started (model detection only)");
        
        // 缓存最新的检测结果，供串口线程使用
        double last_delta_yaw = 0.0;
        double last_delta_pitch = 0.0;
        bool last_control = false;
        bool last_shoot = false;
        
        while (!quit) {
            auto thread_start = std::chrono::steady_clock::now();
            
            try {
                cv::Mat img;
                std::chrono::steady_clock::time_point timestamp;
                
                // 读取摄像头数据
                camera.read(img, timestamp);
                
                if (!img.empty()) {
                    int frame_id = ++global_frame_id;
                    
                    // 将数据推送到队列（仅用于显示）
                    if (image_queue.size() < 2) {
                        image_queue.push({img.clone(), timestamp, frame_id});
                    }
                    camera_frame_count++;
                    
                    // 进行模型检测（不包含串口发送）
                    auto detection_start = std::chrono::steady_clock::now();
                    
                    // 1. YOLO检测
                    auto armors = yolo.detect(img, frame_id);
                    
                    // 2. 目标跟踪
                    auto targets = tracker.track(armors, timestamp);
                    
                    // 3. 设置四元数到solver
                    solver.set_R_gimbal2world({w.load(), x.load(), y.load(), z.load()});
                    
                    // 4. 瞄准计算
                    auto command = aimer.aim(targets, timestamp, 27, false);
                    
                    // 如果没有检测到装甲板，强制设置shoot=0
                    if (targets.empty()) {
                        command.shoot = false;
                        command.control = false;
                    }
                    
                    // 5. 更新缓存的控制指令（供串口线程使用）
                    if (targets.empty() || !command.control) {
                        last_delta_yaw = 0.0;
                        last_delta_pitch = 0.0;
                        last_control = false;
                        last_shoot = false;
                    } else {
                        // 计算当前云台的yaw和pitch角度
                        Eigen::Quaterniond gimbal_q(w.load(), x.load(), y.load(), z.load());
                        Eigen::Vector3d gimbal_ypr = tools::eulers(gimbal_q, 2, 1, 0);
                        double current_yaw = gimbal_ypr[0];
                        double current_pitch = gimbal_ypr[2];  // 使用roll值作为pitch
                        
                        // 计算角度差值
                        last_delta_yaw = command.yaw - current_yaw;
                        last_delta_pitch = command.pitch - current_pitch;
                        
                        // 限制差值范围
                        last_delta_yaw = tools::limit_rad(last_delta_yaw);
                        last_delta_pitch = tools::limit_rad(last_delta_pitch);
                        
                        last_control = command.control;
                        last_shoot = command.shoot;
                    }
                    
                    detection_frame_count++;
                    
                    auto detection_time = std::chrono::duration_cast<std::chrono::microseconds>(
                        std::chrono::steady_clock::now() - detection_start);
                    
                    if (frame_id % 100 == 0) {  // 减少日志频率
                        tools::logger()->info("Detection time: {:.1f}ms", detection_time.count() / 1000.0);
                    }
                }
                
            } catch (const std::exception & e) {
                tools::logger()->warn("Camera read failed: {}", e.what());
                std::this_thread::sleep_for(std::chrono::milliseconds(5));
                continue;
            }
            
            // 控制摄像头读取频率（约100Hz）
            auto thread_end = std::chrono::steady_clock::now();
            auto thread_time = std::chrono::duration_cast<std::chrono::microseconds>(
                thread_end - thread_start);
            
            if (thread_time < std::chrono::microseconds(10000)) { // 10ms间隔
                std::this_thread::sleep_for(std::chrono::microseconds(10000) - thread_time);
            }
        }
        
        tools::logger()->info("Camera thread stopped");
    });



    // 线程2：串口发送线程（高频发送，使用摄像头线程的检测结果）
    auto serial_thread = std::thread([&]() {
        tools::logger()->info("Serial thread started (high frequency)");
        
        // 缓存最新的检测结果，供串口线程使用
        double last_delta_yaw = 0.0;
        double last_delta_pitch = 0.0;
        bool last_control = false;
        bool last_shoot = false;
        
        auto last_time = std::chrono::steady_clock::now();
        
        while (!quit) {
            auto thread_start = std::chrono::steady_clock::now();
            
            // 直接发送角度差值数据（高频发送）
            if (serial_port.isOpen()) {
                try {
                    // 快速打包数据
                    uint8_t frame_data[13] = {
                        0xAA, 2, 1, 
                        static_cast<uint8_t>(last_control ? 1 : 0), 
                        static_cast<uint8_t>(last_shoot ? 1 : 0),
                        static_cast<uint8_t>((int16_t)(last_delta_yaw * 57.3 * 100) & 0xFF),
                        static_cast<uint8_t>(((int16_t)(last_delta_yaw * 57.3 * 100) >> 8) & 0xFF),
                        static_cast<uint8_t>((int16_t)(last_delta_pitch * 57.3 * 100) & 0xFF),
                        static_cast<uint8_t>(((int16_t)(last_delta_pitch * 57.3 * 100) >> 8) & 0xFF),
                        0x30, 0x75, // 固定距离30000
                        254, 0xBB   // 质量+帧尾
                    };
                    
                    serial_port.write(frame_data, 13);
                    serial_frame_count++;
                    
                } catch (const std::exception & e) {
                    // 忽略发送错误
                }
            }
            
            // 控制串口发送频率（500Hz，约2ms间隔）
            auto thread_end = std::chrono::steady_clock::now();
            auto thread_time = std::chrono::duration_cast<std::chrono::microseconds>(
                thread_end - thread_start);
            
            if (thread_time < std::chrono::microseconds(2000)) { // 2ms间隔
                std::this_thread::sleep_for(std::chrono::microseconds(2000) - thread_time);
            }
        }
        
        tools::logger()->info("Serial thread stopped");
    });

    // 主线程：显示和协调
    tools::logger()->info("Display thread started");
    
    cv::Mat img;
    auto t0 = std::chrono::steady_clock::now();
    int frame_count = 0;
    
    while (!exiter.exit()) {
        ImageData image_data;
        if (image_queue.pop(image_data)) {
            img = image_data.image;
            
            if (img.empty()) {
                continue;
            }

            // 性能统计显示（减少显示更新频率）
            if (frame_count % 5 == 0) {
                auto current_time = std::chrono::steady_clock::now();
                double elapsed_seconds = std::chrono::duration<double>(current_time - start_time).count();
                
                double camera_fps = camera_frame_count / elapsed_seconds;
                double detection_fps = detection_frame_count / elapsed_seconds;
                double serial_fps = serial_frame_count / elapsed_seconds;
                double display_fps = display_frame_count / elapsed_seconds;
                
                tools::draw_text(
                    img,
                    fmt::format("Camera FPS: {:.1f}, Detection FPS: {:.1f}", camera_fps, detection_fps),
                    {10, 30}, {0, 255, 0});
                tools::draw_text(
                    img,
                    fmt::format("Serial FPS: {:.1f}, Display FPS: {:.1f}", serial_fps, display_fps),
                    {10, 60}, {0, 255, 0});
            }

            // 显示四元数信息（减少显示频率）
            if (frame_count % 10 == 0) {
                Eigen::Quaterniond gimbal_q(w.load(), x.load(), y.load(), z.load());
                tools::draw_text(
                    img,
                    fmt::format("Gimbal Yaw: {:.2f}", (tools::eulers(gimbal_q.toRotationMatrix(), 2, 1, 0) * 57.3)[0]),
                    {10, 90}, {255, 255, 255});
            }

            cv::resize(img, img, {}, 0.5, 0.5);
            cv::imshow("Parallel Pipeline USB Camera", img);
            auto key = cv::waitKey(1);
            if (key == 'q') break;
            
            display_frame_count++;
            frame_count++;
        } else {
            // 没有图像数据时短暂休眠
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
    }

    // 清理线程
    quit = true;
    if (camera_thread.joinable()) camera_thread.join();
    if (serial_thread.joinable()) serial_thread.join();

    // 关闭串口
    if (serial_port.isOpen()) {
        serial_port.close();
    }

    tools::logger()->info("Simplified YOLO detection pipeline stopped");
    return 0;
}